/*Scott Rogowski and Yipeng Huang*/
/*smr2167 yh2315*/

#include "misc.h"
#include "mipspipe.h"
#include "params.h"


/*********************************************************************************************/
/*********************************************UPDATE QUEUES***********************************/
/*********************************************************************************************/

//Update the expected value queues.  The next expected value will therefore be the values at data 0.
void update_queues() {
	
	//handle the incoming data bits
	//if we are writing and valid has been asserted, set the persistent data memory at the incoming address to the incoming value
	if (data_rw[0]==1 && data_valid[0]==1) {
		mem_valid[data_addr[0]]=1;
		mem_data[data_addr[0]] = data_value[0];
		}
	
	//handle the incoming instruction bits
	//TODO, do we need to handle these bits?
	
	//handle the incoming register bits
	//if we are writing, set the register at the incoming address to the incoming value
	if (reg_write_q[0]==1) {
		reg_valid[reg_addr_q[0]] = 1;
		reg_data[reg_addr_q[0]] = reg_data_q[0];
		}
	
	
	/*DECREMENT THE QUEUES*/
	
	//data memory valid
	for (i=0; i<4; i++)
		data_valid[i]=data_valid[i+1];
	data_valid[4]=0;

	//data memory rw
	for (i=0; i<4; i++)
		data_rw[i]=data_rw[i+1];
	data_rw[4]=0;

	//data memory addr	
	for (i=0; i<4; i++)
		data_addr[i]=data_addr[i+1];
	data_addr[4]=0;

	//data memory value	
	for (i=0; i<4; i++)
		data_value[i]=data_value[i+1];
	data_value[4]=0;


	//instruction valid	
	for (i=0; i<4; i++)
		inst_valid[i]=inst_valid[i+1];
	inst_valid[4]=1;

	//instruction value
	for (i=0; i<4; i++)
		inst_addr[i]=inst_addr[i+1];
	inst_addr[4]=inst_addr[3]+4;
	
	
	//register data
	for (i=0; i<4; i++)
		reg_data_q[i]=reg_data_q[i+1];
	reg_data_q[4]=-1;
	
	//register address
	for (i=0; i<4; i++)
		reg_addr_q[i]=reg_addr_q[i+1];
	reg_addr_q[4]=-1;
	
	//register write (boolean)
	for (i=0; i<4; i++)
		reg_write_q[i]=reg_write_q[i+1];
	reg_write_q[4]=0;
	
	//Also, increment the instructions.  This is a hack but should work 
	//inst_addr[4] = inst_addr[3]+4;
	
	}
	
/*********************************************************************************************/
/***********************CLEAR PINS BEFORE REWRITING ON THEM***********************************/
/*********************************************************************************************/
	
void clear_input_ports() {
	ports_i.reset=0;
	ports_i.inst_i_valid=0;
	ports_i.inst_i=0;
	ports_i.data_i_valid=0;
	ports_i.data_i=0;
	}

//Reset to a 0 state
void initialize_values() {
	
	inst_valid[0] = 0;
	inst_valid[1] = 0;
	inst_valid[2] = 1;
	inst_valid[3] = 1;
	inst_valid[4] = 1;
	
	inst_addr[0] = 0;
	inst_addr[1] = 0; //TODO: there shouldn't be an extra cycle asking for 0
	inst_addr[2] = 0;
	inst_addr[3] = 4;
	inst_addr[4] = 8;
	
	memset(&reg_data, 0, 32 * sizeof(int));
	memset(&reg_valid, 0, 32 * sizeof(int));
	
	memset(&mem_data, 0, 65536 * sizeof(int));
	memset(&mem_valid, 0, 65536 * sizeof(int));

	memset(&data_valid, 0, 5 * sizeof(int));
	memset(&data_rw, 0, 5 * sizeof(int));
	memset(&data_addr, 0, 5 * sizeof(int));
	memset(&data_value, 0, 5 * sizeof(int));
	
	memset(&reg_data_q, -1, 5 * sizeof(int));
	memset(&reg_addr_q, -1, 5 * sizeof(int));
	
}



	
/*********************************************************************************************/
/************************CHECK WHETHER RETURNED VALUES ARE CORRECT****************************/
/*********************************************************************************************/

//Check that the instruction is what we were expecting
int process_instruction_request() {
	if (params.verbose) 
		vpi_printf
		    ("\n%9lld : Instruction request (expInstValid:%d dutInstValid:%d expInstAddr:0x%x dutInstAddr:0x%x)\n",
		     params.cycles,
		     inst_valid[0], ports_o.inst_o_valid, 
		     inst_addr[0], ports_o.inst_o_addr);

	  
	if (	/*ports_o.inst_o_valid == inst_valid[0]
			&&*/ ports_o.inst_o_addr == inst_addr[0]) 
			return 2;
	else
			return 0;
	}
	

int process_data_request() {
	if (params.verbose) 
		vpi_printf
		    ("%9lld : Data write/read (expDataValid:%d dutDataValid:%d expDataAddr:0x%x dutDataAddr:0x%x expDataValue:0x%x dutDataValue:0x%x expDataRW:0x%d dutDataRW:0x%d)\n",
		     params.cycles,
		     data_valid[0], ports_o.data_o_valid,
		     data_addr[0], ports_o.data_o_addr,
		     data_value[0], ports_o.data_o_value,
		     data_rw[0], ports_o.data_o_rw
		      );
	if (	data_valid[0] == 0 || //just ignore this if we weren't expecting a data valid
			(ports_o.data_o_valid == data_valid[0]
			&& ports_o.data_o_addr == data_addr[0]
			&& ports_o.data_o_value == data_value[0]
			&& ports_o.data_o_rw == data_rw[0])) 
			return 2;
	else
			return 0;
	}



/*********************************************************************************************/
/*********************CALCULATE FUTURE VALUES BASED ON INSTRUCTION****************************/
/*********************************************************************************************/

void add(int rs, int rt, int rd) {
	
	//For now, set the additive values to -1
	int rsVal=-1;
	int rtVal=-1;
	
	//check to see whether we will be reading from any of the registers we will be writing to in the future.
	//If we are, preemptively make that our register value for prediction purposes
	//Start with the future 4 and work backwards.
	for (i=4; 0<=i; i--) {
		if (rsVal==-1 && rs==reg_addr_q[i]) {
			rsVal=reg_data_q[i];
			}
		if (rtVal==-1 && rt==reg_addr_q[i]) {
			rtVal=reg_data_q[i];
			}
		}
	
	//If we are not using the future registers, just use the current registers
	if (rsVal==-1) rsVal = reg_data[rs];
	if (rtVal==-1) rtVal = reg_data[rt];
	
	//Add into the future
	reg_addr_q[4]=rd;
	reg_data_q[4]=(rs+rt) & 0xffffffffffffffff; //To handle overflow situations correctly, mask it.
	reg_write_q[4]=1;
	
	if (params.verbose) {
		vpi_printf("%9lld : Writing ADD instruction:  0x%x; $%x = $%x + $%x; $%x = %x\n",
			   params.cycles,
			   ports_i.inst_i,
			   rd,rs,rt,
			   rd,reg_data_q[4]
			   );
		}
		
	}

//$rt = $rs + $immediate
void addi(int rs, int rt, int immediate) {
	
	//For now, set the additive values to -1
	int rsVal=-1;
	
	//check to see whether we will be reading from any of the registers we will be writing to in the future.
	//If we are, preemptively make that our register value for prediction purposes
	//Start with the future 4 and work backwards.
	for (i=4; 0<=i; i--) {
		if (rs==reg_addr_q[i]) {
			rsVal=reg_data_q[i]; 
			break;
			}
		}
	
	//If we are not using the future registers, just use the current registers
	if (rsVal==-1) rsVal = reg_data[rs];
	
	//Add immediate into the future
	reg_addr_q[4]=rt;
	reg_data_q[4]=(immediate+rsVal) & 0xffffffffffffffff; //To handle overflow situations correctly, mask it.
	reg_write_q[4]=1;
	
	
	if (params.verbose) {
		vpi_printf("%9lld : Writing ADDI instruction: 0x%x; $%x = $%x + %x; $%x = %x;\n",
			   params.cycles,
			   ports_i.inst_i,
			   rt,rs,immediate,
			   rt,reg_data_q[4]);
		}
	
	
	
	}

//MEM[$rs+immediate]=$rt
void sw(int rs, int rt, int immediate) {
	//For now, set the additive values to -1
	int rtVal=-1;
	int rsVal=-1;
	
	//check to see whether we will be reading from any of the registers we will be writing to in the future.
	//If we are, preemptively make that our register value for prediction purposes
	//Start with the future 4 and work backwards.
	for (i=4; 0<=i; i--) {
		if (rtVal==-1 && rt==reg_addr_q[i]) {
			rtVal=reg_data_q[i];
			}
		if (rsVal==-1 && rs==reg_addr_q[i]) {
			rsVal=reg_data_q[i];
			}
		}
	
	//If we are not using the future registers, just use the current registers
	if (rtVal==-1) rtVal = reg_data[rt];
	if (rsVal==-1) rsVal = reg_data[rs];
	
	//Store word in the future
	//TODO.  I think this is a hack.  The data stage should be 3 long correct?
	data_addr[4]=(rsVal+immediate) & 0xffffffffffffffff;//To handle overflow situations correctly, mask it.
	data_value[4]=rtVal;
	data_valid[4]=1;
	data_rw[4]=1;
	
	
	if (params.verbose) {
		vpi_printf("%9lld : Writing SW instruction: 0x%x; MEM[$%x+%x] = $%x; MEM[%x] = %x\n",
			   params.cycles,
			   ports_i.inst_i,
			   rs, immediate, rt,
			   data_addr[4], data_value[4]);
		}
	
	
	
	}

// $t=MEM[$s+offset]
void lw(int rs, int rt, int immediate) {
	//For now, set the additive values to -1
	int rsVal=-1;
	
	//check to see whether we will be reading from any of the registers we will be writing to in the future.
	//If we are, preemptively make that our register value for prediction purposes
	//Start with the future 4 and work backwards.
	for (i=4; 0<=i; i--) {
		if (rs==reg_addr_q[i]) {
			rsVal=reg_data_q[i];
			break;
			}
		}
	
	//If we are not using the future registers, just use the current registers
	if (rsVal==-1) rsVal = reg_data[rs];
	
	//We can now calculate the memory fetch address
	int dataAddr=(rsVal+immediate) & 0xffffffffffffffff;
	
	//Do the same 'check for future write' thing for the data memory we will be requesting...
	int valMem=-1;
	
	//check to see whether we will be writing to any of the data memory which we will be reading in the future.
	//If we are, preemptively make that our data memory value
	//Start with the future 4 and work backwards.
	for (i=2; 0<=i; i--) {
		if (dataAddr==data_addr[i]) valMem=data_value[i];
		}
	if(valMem==-1) valMem=mem_data[dataAddr];
	
	//Write to the register in the future
	reg_addr_q[4]=rt;
	reg_data_q[4]=valMem; //To handle overflow situations correctly, mask it.
	reg_write_q[4]=1;
	
	//Read from  the data memory in the future
	data_addr[2]=dataAddr;
	data_valid[2]=1;
	data_rw[2]=0;
	
	if (params.verbose) {
		vpi_printf("%9lld : Writing LW instruction: 0x%x; $%x = MEM[$%x+%x]; $%x = MEM[%x]; $%x = %x\n",
			   params.cycles,
			   ports_i.inst_i,
			   rt, rs, immediate,
			   rt, dataAddr,
			   rt, valMem
			   );
		}
	
	
	}
	
void beq(int rs, int rt, int immediate) {

	//For now, assume the register values are -1
	int rsVal=-1;
	int rtVal=-1;
	
	//check to see whether we will be reading from any of the registers we will be writing to in the future.
	//If we are, preemptively make that our register value for prediction purposes
	//Start with the future 4 and work backwards.
	for (i=4; 0<=i; i--) {
		if (rsVal==-1 && rs==reg_addr_q[i]) {
			rsVal=reg_data_q[i];
			}
		if (rtVal==-1 && rt==reg_addr_q[i]) {
			rtVal=reg_data_q[i];
			}
		}
	
	//If we are not using the future registers, just use the current registers
	if (rsVal==-1) rsVal = reg_data[rs];
	if (rtVal==-1) rtVal = reg_data[rt];
	
	//now, branch if rs rt are equal
	if (rsVal == rtVal) {
		
		/*inst_addr[0] = 0; //BEQ immediately shuts off inst_o_valid in the mips pipe, so we should get no instruction requests for the next three cycles.
		inst_addr[1] = 0;
		inst_addr[2] = 0;*/
		inst_addr[3] = inst_addr[0] + immediate; //TODO: make sure that inst_addr[0] gives us the PC counter.
		//BEQ adds to the program counter the signed value of immediate, with the last two bits truncated.
		
		inst_valid[0] = 0;
		inst_valid[1] = 0;
		inst_valid[2] = 0;
		inst_valid[3] = 1;
		
		}
		
	if (params.verbose) {
		vpi_printf("%9lld : Writing BEQ instruction: 0x%x; if ($%x==$%x) then jump %x; if (%x==%x) then jump %x;\n",
			   params.cycles,
			   ports_i.inst_i,
			   rs,rt,immediate,
			   rsVal,rtVal,immediate);
		}
	
	
	}

void jmp(int target) {
	
	int decodedTarget = inst_addr[0] & 0xF0000000 + target<<2;
	
	/*inst_addr[0] = 0; //jump immediately shuts off inst_o_valid in the mips pipe, so we should get no instruction requests for the next three cycles.
	inst_addr[1] = 0;
	inst_addr[2] = 0;*/
	inst_addr[3] = decodedTarget;  //we expect the jump address to come back as the requested address three cycles from now.
	
	inst_valid[0] = 0;
	inst_valid[1] = 0;
	inst_valid[2] = 0;
	inst_valid[3] = 1;
	
	if (params.verbose) {
		vpi_printf("%9lld : Writing JMP instruction: 0x%x; jump %x;\n",
			   params.cycles,
			   ports_i.inst_i,
			   decodedTarget);
		}
	
	
	}


/*********************************************************************************************/
/***************************DRIVE NEW INSTRUCTION (RANDOM & MASKED)***************************/
/*********************************************************************************************/

void set_instruction() {
	//If we are not ready to give an instruction, don't give it
	if (!ports_o.inst_o_valid) return;

	//For working out probabalistically which instruction to send next
	float chooseInst = rand()/((float)RAND_MAX);
	float probCovered = 0;
	
	//The instruction components
	int opcode, rs, rt, rd, sh, func, immediate, target;
	
	//The instruction
	long instruction;
	
	//vpi_printf("Choose Instruction: %f\n",chooseInst);
	/***************************************************************************/
	/*Depending on which instruction was decided upon, set the instruction bits*/
	/***************************************************************************/
		
	//add
	if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_add)) {
		opcode = 0b000000;
		
		rs = rand()%32 & params.reg_read_index_mask;
		rt = rand()%32 & params.reg_read_index_mask;
		rd = rand()%32 & params.reg_write_index_mask;
		
		sh = 0b00000; //not used by add but still required in the instruction
		
		func = 0b100000;//the add func code
		
		instruction = opcode<<26 | rs<<21 | rt<<16 | rd<<11 | sh<<6 | func;
		ports_i.inst_i = instruction;
		
		vpi_printf("%9lld : Creating ADD instruction: 0x%x\n",
			   params.cycles,
			   instruction);
			
		add(rs,rt,rd);
		}
		
	//addi
	else if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_addi)) {
		opcode = 0b001000;

		rs = rand()%32 & params.reg_read_index_mask;
		rt = rand()%32 & params.reg_write_index_mask;

		immediate = rand()%65536 & params.addi_mask;
		
		instruction = opcode<<26 | rs<<21 | rt<<16 | immediate;
		
		/*
		vpi_printf("%9lld : opcode shifted: 0x%x\n",
			   params.cycles,
			   opcode<<26);
		vpi_printf("%9lld : rs shifted: 0x%x\n",
			   params.cycles,
			   rs<<21);
		vpi_printf("%9lld : rt shifted: 0x%x\n",
			   params.cycles,
			   rt<<16);
		vpi_printf("%9lld : immediate: 0x%x\n",
			   params.cycles,
			   immediate);*/
		
		ports_i.inst_i = instruction;
		
		vpi_printf("%9lld : Creating ADDI instruction: 0x%x\n",
			   params.cycles,
			   instruction);
			
		addi(rs,rt,immediate);
		}

	//sw
	else if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_sw)) {
		opcode = 0b101011;

		rs = rand()%32 & params.reg_read_index_mask;
		rt = rand()%32 & params.reg_write_index_mask;


		immediate = rand()%65536 & params.data_write_index_mask;

		instruction = opcode<<26 | rs<<21 | rt<<16 | immediate;

		ports_i.inst_i = instruction;
		
		vpi_printf("%9lld : Creating SW instruction: 0x%x\n",
			   params.cycles,
			   instruction);
		
		sw(rs,rt,immediate);
		}
	
	//lw
	else if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_lw)) {
		opcode = 0b100011;

		rs = rand()%32 & params.reg_read_index_mask;
		rt = rand()%32 & params.reg_write_index_mask;

		immediate = rand()%65536 & params.data_read_index_mask;
		
		instruction = opcode<<26 | rs<<21 | rt<<16 | immediate;		
			ports_i.inst_i = instruction;
			
			vpi_printf("%9lld : Creating LW instruction: 0x%x\n",
				   params.cycles,
				   instruction);
			
		lw(rs,rt,immediate);
		}
	
	//beq
	else if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_beq)) {
		opcode = 0b000100;

		rs = rand()%32 & params.reg_read_index_mask;
		rt = rand()%32 & params.reg_read_index_mask;

		immediate = rand()%65536;
		
		instruction = opcode<<26 | rs<<21 | rt<<16 | immediate;
			ports_i.inst_i = instruction;
			
			vpi_printf("%9lld : Creating BEQ instruction: 0x%x\n",
				   params.cycles,
				   instruction);
			
		beq(rs, rt, immediate);
		}
	
	//jmp
	else if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_jmp)) {
		opcode = 0b000010;

		target = rand()%67108864;
		
		instruction = opcode<<26 + target;
			ports_i.inst_i = instruction;
			
			vpi_printf("%9lld : Creating JMP instruction: 0x%x\n",
				   params.cycles,
				   instruction);
			
		jmp(target);
		}
	
	//noop
	else if (probCovered<=chooseInst && chooseInst<(probCovered+=params.density_noop)) {
		
		//Generage a random opcode and repeat if it is any one of our six supported opcodes
		do {
			opcode = rand() & 0b111111;
			} while (opcode == 0b000010 || opcode == 0b000100 || opcode == 0b100011 || opcode == 0b101011 || opcode == 0b001000 || opcode == 0b000000);

		target = 0;
		//target = rand()%26;		

		instruction = opcode<<26 + target;
		
		vpi_printf("%9lld : Creating NOOPCreating instruction: 0x%x\n",
			   params.cycles,
			   instruction);
		
		//noop();
		}

	//Finally, set the actual the ports
	//ports_i.inst_i = instruction;
	ports_i.inst_i_valid = 1;
	

	
	}


/*********************************************************************************************/
/************************READ/WRITE DATA MEMORY. DRIVE BITS IF READING************************/
/*********************************************************************************************/

void set_data() {
	//If the valid signal is 1, then we are manipulating the data
	if (ports_o.data_o_valid==1) {
		//Assume we are returning the value immediately (as we are for now)
		ports_i.data_i_valid = 1;
		//If there is a read request
		
		if (ports_o.data_o_rw == 0) {
			ports_i.data_i = mem_data[ports_o.data_o_addr]; //TODO should this happen here or elsewhere since we are supposed to wait a cycle??  Update: I think it should be here because we are now waiting for 1 cycle in bench.v
			if (params.verbose) {
				vpi_printf("%9lld : Fetching data memory: 0x%x from address 0x%x \n",
						params.cycles,
						ports_i.data_i,
						ports_o.data_o_addr);
				}
			}
		//Otherwise, we have a write request
		else {
			//Do the write //TODO and with the read requests, this is also being done in update queue
			mem_data[ports_o.data_o_addr]=ports_o.data_o_value;
			if (params.verbose) {
				vpi_printf("%9lld : Writing data memory 0x%x to address 0x%x\n",
						params.cycles,
						ports_o.data_o_value,
						ports_o.data_o_addr);
				}
			}
		}
		
	}


/*********************************************************************************************/
/******************RESET ALL QUEUES AND INTERNAL MEMORY AND SET RESET PIN *********************/
/*********************************************************************************************/

//Random & masked
void set_reset() {
	
	if (!ports_i.reset)
		return;
	
	if (rand() % 100 < 100 * params.density_reset) {
		ports_i.reset = 1;
		
		initialize_values();

		if (params.verbose) 
			vpi_printf("%9lld : Reset\n", params.cycles);
		}
	}


